/**
 * @file car_motion_control.cpp
 * @author huangwei (you@domain.com)
 * @brief Subscribe cmd_vel and joint states,then caculate and publish all motor velocity
 * @version 0.1
 * @date 2024-05-30
 * 
 * @copyright Copyright (c) 2024
 * 
 */
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
#include <iostream>
#include <math.h>

double direction_angel[4];
double driver_vel[4];
double direction_action_pos[4];


/**
 * @brief Caculate the direction position and velocity,follow the ikine
 * 
 * @param cmd_vel 
 * @param direction 
 * @param driver 
 */
void twistToDirectionMotor(const geometry_msgs::Twist &cmd_vel,double(&direction)[4],double(&driver)[4] )
{
	double rx = 0.260; // half length of front to back axis
	double ry = 0.2;   // half width of front two wheel
	direction[0] = atan2((cmd_vel.linear.y + cmd_vel.angular.z * rx), (cmd_vel.linear.x - cmd_vel.angular.z * ry)) ;
	direction[1] = atan2((cmd_vel.linear.y - cmd_vel.angular.z * rx), (cmd_vel.linear.x - cmd_vel.angular.z * ry)) ;
	direction[2] = atan2((cmd_vel.linear.y - cmd_vel.angular.z * rx), (cmd_vel.linear.x + cmd_vel.angular.z * ry));
	direction[3] = atan2((cmd_vel.linear.y + cmd_vel.angular.z * rx), (cmd_vel.linear.x + cmd_vel.angular.z * ry)) ;
	ROS_INFO("expected postion %lf    %lf      %lf     %lf ",direction[0],direction[1],direction[2],direction[3]);

	driver[0] = cmd_vel.linear.x * cos(direction[0]);
	driver[1] = cmd_vel.linear.x * cos(direction[1]);
	driver[2] = cmd_vel.linear.x * cos(direction[2]);
    driver[3] = cmd_vel.linear.x * cos(direction[3]);

}

/**
 * @brief cmd_vel callback function
 * 
 * @param cmd_vel 
 */
void cmd_callback(const geometry_msgs::Twist &cmd_vel)
{
	// ROS_INFO("Received a /cmd_vel message!");
	// ROS_INFO("Linear Components:[%f,%f,%f]", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.linear.z);
	// ROS_INFO("Angular Components:[%f,%f,%f]", cmd_vel.angular.x, cmd_vel.angular.y, cmd_vel.angular.z);
	twistToDirectionMotor(cmd_vel,direction_angel,driver_vel);
}

/**
 * @brief joint states callback function
 * 
 * @param joint_states 
 */
void state_callback(const sensor_msgs::JointState &joint_states)
{//name: [LB_J1, LB_J2, LF_J1, LF_J2, RB_J1, RB_J2, RF_J1, RF_J2]
	direction_action_pos[0] = joint_states.position[2];
	direction_action_pos[1] = joint_states.position[0];
	direction_action_pos[2] = joint_states.position[4];
	direction_action_pos[3] = joint_states.position[6];

	// ROS_INFO("Received a /joint state message!");
	ROS_INFO("actual postion %lf    %lf      %lf     %lf ",direction_action_pos[0],direction_action_pos[1],direction_action_pos[2],direction_action_pos[3]);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "cmd_vel_listener");
	ros::NodeHandle n;
	ros::Subscriber cmd_sub = n.subscribe("/cmd_vel", 20, cmd_callback);					  //订阅cmd_vel话题
	ros::Subscriber state_sub = n.subscribe("/newcar5/joint_states", 20, state_callback); //订阅cmd_vel话题

	ros::Publisher LF1_commad_pub = n.advertise<std_msgs::Float64>("/newcar5/LF1_position_controller/command",10);
	ros::Publisher LF2_commad_pub = n.advertise<std_msgs::Float64>("/newcar5/LF2_position_controller/command",10);
	ros::Publisher LB1_commad_pub = n.advertise<std_msgs::Float64>("/newcar5/LB1_position_controller/command",10);
	ros::Publisher LB2_commad_pub = n.advertise<std_msgs::Float64>("/newcar5/LB2_position_controller/command",10);
	ros::Publisher RF1_commad_pub = n.advertise<std_msgs::Float64>("/newcar5/RF1_position_controller/command",10);
	ros::Publisher RF2_commad_pub = n.advertise<std_msgs::Float64>("/newcar5/RF2_position_controller/command",10);
	ros::Publisher RB1_commad_pub = n.advertise<std_msgs::Float64>("/newcar5/RB1_position_controller/command",10);
	ros::Publisher RB2_commad_pub = n.advertise<std_msgs::Float64>("/newcar5/RB2_position_controller/command",10);
   
	ros::Rate rate(20);
    while (ros::ok())
	{
		std_msgs::Float64 LF2_vel;
		LF2_vel.data = driver_vel[0];
		LF2_commad_pub.publish(LF2_vel);

		std_msgs::Float64 LB2_vel;
		LB2_vel.data = driver_vel[1];
		LB2_commad_pub.publish(LB2_vel);

		std_msgs::Float64 RB2_vel;
		RB2_vel.data = -driver_vel[2];
		RB2_commad_pub.publish(RB2_vel);

		std_msgs::Float64 RF2_vel;
		RF2_vel.data = -driver_vel[3];
		RF2_commad_pub.publish(RF2_vel);

		std_msgs::Float64 LF1_vel;//name: [LB_J1, LB_J2, LF_J1, LF_J2, RB_J1, RB_J2, RF_J1, RF_J2]
		LF1_vel.data = direction_angel[0] - direction_action_pos[0];
		LF1_commad_pub.publish(LF1_vel);

		std_msgs::Float64 LB1_vel;
		LB1_vel.data = direction_angel[1] - direction_action_pos[1];
		LB1_commad_pub.publish(LB1_vel);

		std_msgs::Float64 RB1_vel;
		RB1_vel.data = direction_angel[2] - direction_action_pos[2];
		RB1_commad_pub.publish(RB1_vel);

 		std_msgs::Float64 RF1_vel;
		RF1_vel.data = direction_angel[3] - direction_action_pos[3];
		RF1_commad_pub.publish(RF1_vel);

        ros::spinOnce();
		rate.sleep();

		
	}
	
	//  ros::spin();

	return 1;
}